#include "skywatcher.h"
#include "usart.h"

/* ----------------------------------------------------------------------- */
/* Bits de configuration: adapté à votre dispositif et à vos besoins */

//code char at __CONFIG1H CONFIG1H = _OSC_HS_1H & _FCMEN_ON_1H & _IESO_ON_1H;
code char at __CONFIG1H CONFIG1H = _OSC_HSPLL_1H & _FCMEN_ON_1H & _IESO_ON_1H;
code char at __CONFIG2L CONFIG2L = _PWRTEN_ON_2L & _BOREN_ON_2L & _BORV_20_2L;
code char at __CONFIG2H CONFIG2H = _WDTEN_OFF_2H & _WDPS_32768_2H & _WINEN_OFF_2H;
code char at __CONFIG3L CONFIG3L = _PWMPIN_OFF_3L & _LPOL_HIGH_3L & _HPOL_HIGH_3L & _T1OSCMX_ON_3L;
code char at __CONFIG3H CONFIG3H = _FLTAMX_RC1_3H & _SSPMX_RC7_3H & _PWM4MX_RB5_3H & _EXCLKMX_RC3_3H & _MCLRE_ON_3H;
code char at __CONFIG4L CONFIG4L = _STVREN_ON_4L & _LVP_ON_4L & _DEBUG_OFF_4L;
code char at __CONFIG5L CONFIG5L = _CP0_OFF_5L & _CP1_OFF_5L & _CP2_OFF_5L & _CP3_OFF_5L;
code char at __CONFIG5H CONFIG5H = _CPB_OFF_5H & _CPD_OFF_5H;
code char at __CONFIG6L CONFIG6L = _WRT0_OFF_6L & _WRT1_OFF_6L & _WRT2_OFF_6L & _WRT3_OFF_6L;
code char at __CONFIG6H CONFIG6H = _WRTC_OFF_6H & _WRTB_OFF_6H & _WRTD_OFF_6H;
code char at __CONFIG7L CONFIG7L = _EBTR0_OFF_7L & _EBTR1_OFF_7L & _EBTR2_OFF_7L & _EBTR3_OFF_7L;
code char at __CONFIG7H CONFIG7H = _EBTRB_OFF_7H;

//resolution 9 bits PWMFREQ=78125hz@40Mhz 
unsigned int pwm_table[] = {0x0000, 0x0071, 0x00de, 0x013f, 0x0190, 0x01cd, 0x01f3, 0x01ff}; 
#define PWMFULL 0x1ff
/* PWMPERIOD=12800ns i.e. 128xTIMERPERIOD: for low speeds, step_delay should be at least 16xPWMPERIOD */

unsigned long ra_position = 0x800000;
unsigned char ra_microstep = 0x00;
unsigned char ra_pwmindex = MICROSTEPS - 1;
unsigned long ra_wormperiod = 0x256;
unsigned long ra_target = 0x000001; 
unsigned long ra_target_current = 0x000000;
unsigned long ra_target_slow = 400;
unsigned long ra_breaks = 400;
TimerValue ra_timer;
speedRampData ra_speedramp;

unsigned long de_position = 0x800000;
unsigned char de_microstep = 0x00;
unsigned char de_pwmindex = MICROSTEPS - 1;
unsigned long de_wormperiod = 0x256;
unsigned long de_target = 0x800000;
unsigned long de_target_current = 0x000000;
unsigned long de_target_slow = 400;
unsigned long de_breaks = 200;
TimerValue de_timer;
speedRampData de_speedramp;

/* Motor status (Skywatcher protocol) */
unsigned int ra_status=0X0010; // lowspeed, forward, slew mode, stopped
unsigned int de_status=0X0010; // lowspeed, forward, slew mode, stopped

/** Initialisations **/

void init_pwm(void) {
  PTCON0 = 0x00; // postscaler = 1:1 (0000); prescaler= 1:1 (00); Free running mode (00)
  PTCON1 = 0x00; // PTMR off (0); PTMR direction upwards (0); unused (000000)
  // PWMCON0 = 0x6F; // unused (0); pwm1&3 enabled (0110); all independent pairs (1111)
  PWMCON0 = 0x7F; // unused (0); pwm1&3&5&7 enabled (0111); all independent pairs (1111)
  PWMCON1 = 0x00; //??
  //    PTPERL = 0xff; // Fpwm = FOSC / (4 * (PTPER + 1))= 19531.25 Hz
  //    PTPERL = 0x3f; // Fpwm = 78125Hz@20Mhz 8bits resolution, 156250Hz@40Mhz
  PTPERL = 0x7f; // Fpwm = 78125Hz@40Mhz 9bits resolution
  //    PTPERL = 0x1f; // Fpwm = 156250Hz
  PTPERH = 0x00;

  //Start PWM
  PTCON1bits.PTEN=1;
}

void init_timers(void) {
  // Init TMR1
  T1CON = 0x04; // 2x8bits mode (RD16=0), TMR1RUN is read only (0), prescale = 1 (00), 
                // use internal clock (000), don't run yet (TMR1ON = 0)  
  // Init TMR0
  T0CON = 0x08; // 2x8bits mode (T016=0), no prescaler (PSA=1), prescale = xxx (xxx), 
                // use internal clock (TOCS=0), don't run yet (TMR0ON = 0)  
}

/* Motor Start/Stop */

void compute_timer_ra(unsigned long wormperiod) {
  unsigned long n = (wormperiod * MUL_RA) ;
  n += ((wormperiod * REM_RA) / RA_STEPS_WORM);
  n = n * TICKS_MICRO;
  T1CONbits.TMR1ON = 0;
  if (n == 0) { ra_timer.count=1; ra_timer.timer.value=MIN_TICKS; }
  else if (n > 65535) { ra_timer.count = (n / 65536) + 1; ra_timer.timer.value = 65536 - (n / ra_timer.count); }
  else {  ra_timer.count = 1; if (n < MIN_TICKS) n = MIN_TICKS;  n = 65536 - n; ra_timer.timer.value = n;}
   ra_timer.current=ra_timer.count-1; T1CONbits.TMR1ON = 1;
}

void compute_timer_de(unsigned long wormperiod) {
  unsigned long n = (wormperiod * MUL_DE) ;
  n += ((wormperiod * REM_DE) / DE_STEPS_WORM);
  n = n * TICKS_MICRO;

  if (n == 0) { de_timer.count=1; de_timer.timer.value=MIN_TICKS; }
  else if (n > 65535) { de_timer.count = (n / 65536) + 1; de_timer.timer.value = 65536 - (n / de_timer.count); }
  else {  de_timer.count = 1; if (n < MIN_TICKS) n = MIN_TICKS; n = 65536 - n; de_timer.timer.value = n;}
}

void ra_resume() {
  compute_timer_ra(ra_wormperiod);
  ra_timer.current=0;
 
  // Acceleration phase
  //if (ra_timer.count > 1) { ra_timer.count=1; ra_timer.value=0; } // lowest possible high speed
  ra_speedramp.min_delay= (65536 - ra_timer.timer.value) * ra_timer.count;
  ra_speedramp.step_delay = C0_APPROX;
  ra_timer.count=C0_TIMER_COUNT;
  ra_timer.timer.fields.valueh=C0_TIMER_VALUEH;
  ra_timer.timer.fields.valuel=C0_TIMER_VALUEL;
  SETTIMER1(ra_timer.timer);
  ra_speedramp.rest=0;
  ra_speedramp.accel_count=0;
  ra_speedramp.run_state = ACCEL;
  
  if GETMOTORPROPERTY(ra_status, HIGHSPEED) {
      SETRAPWMA(PWMFULL);
      SETRAPWMB(PWMFULL);
    } else {
      ra_timer.count=1;
      ra_speedramp.step_delay = C0_APPROX / 8;
  }
  if (!(GETMOTORPROPERTY(ra_status, SLEWMODE))) ra_target_current = 0;

  SETMOTORPROPERTY(ra_status, RUNNING);
  T1CONbits.TMR1ON = 1;
}

void de_resume() {
  compute_timer_de(de_wormperiod);
  de_timer.current=0;
  // Acceleration phase
  de_speedramp.min_delay= (65536 - de_timer.timer.value) * de_timer.count;
  de_speedramp.step_delay = C0_APPROX;
  de_timer.count=C0_TIMER_COUNT;
  de_timer.timer.fields.valueh=C0_TIMER_VALUEH;
  de_timer.timer.fields.valuel=C0_TIMER_VALUEL;
  SETTIMER0(de_timer.timer);
  de_speedramp.rest=0;
  de_speedramp.accel_count=0;
  de_speedramp.run_state = ACCEL;

  if GETMOTORPROPERTY(de_status, HIGHSPEED) {
      SETDEPWMA(PWMFULL);
      SETDEPWMB(PWMFULL);
    } else {
      de_timer.count=1;
      de_speedramp.step_delay = C0_APPROX / 8;
  }
  if (!(GETMOTORPROPERTY(de_status, SLEWMODE))) de_target_current = 0;

  SETMOTORPROPERTY(de_status, RUNNING);
  T0CONbits.TMR0ON = 1;
}

void ra_pause() {
  if ((!(GETMOTORPROPERTY(ra_status, HIGHSPEED))) && (ra_wormperiod > 19)) {
    // stop TMR1
    T1CONbits.TMR1ON = 0;
    UNSETMOTORPROPERTY(ra_status, RUNNING);
  } else { 
    ra_speedramp.accel_count = 0;
    ra_speedramp.rest = 0;
    ra_speedramp.decel_start = ra_breaks;
    ra_speedramp.run_state = DECEL;
  }
  
}

void ra_stop() {
  // stop TMR1
  T1CONbits.TMR1ON = 0;
  UNSETMOTORPROPERTY(ra_status, RUNNING);
}

void de_pause() {
  if ((!(GETMOTORPROPERTY(de_status, HIGHSPEED))) && (de_wormperiod > 19)) {
    // stop TMR0
    T0CONbits.TMR0ON = 0;
    UNSETMOTORPROPERTY(de_status, RUNNING);
  } else { 
    de_speedramp.accel_count = 0;
    de_speedramp.rest = 0;
    de_speedramp.decel_start = de_breaks;
    de_speedramp.run_state = DECEL;
  }
  // stop TMR0
  //T0CONbits.TMR0ON = 0;
  //UNSETMOTORPROPERTY(de_status, RUNNING);
}

void de_stop() {
  // stop TMR0
  T0CONbits.TMR0ON = 0;
  UNSETMOTORPROPERTY(de_status, RUNNING);
}

/*** Interrupts ***/
/** High Priority Interrupt Subroutine **/
void high_isr (void) interrupt 1 {
  unsigned char step_inc;
  if (PIR1bits.TMR1IF==1) { 
    ra_timer.current += 1;
    SETTIMER1(ra_timer.timer); 
    if (ra_timer.current == ra_timer.count) {
      ra_timer.current=0;
      /* if  ((!(GETMOTORPROPERTY(ra_status, SLEWMODE))) && (ra_target_current >= ra_target)) {
	T1CONbits.TMR1ON = 0;
	UNSETMOTORPROPERTY(ra_status, RUNNING);
	}*/
      if  ((!(GETMOTORPROPERTY(ra_status, SLEWMODE))) && (ra_target_current >= ra_target - ra_target_slow) && (ra_speedramp.run_state != DECEL)) {
        ra_speedramp.accel_count = 0;
        ra_speedramp.rest = 0;
	if (GETMOTORPROPERTY(ra_status, HIGHSPEED))
	  ra_speedramp.decel_start = (ra_target_slow / 8);
	else
	  ra_speedramp.decel_start = ra_target_slow;
	ra_speedramp.run_state = DECEL;
      }
      if (ra_speedramp.run_state == ACCEL) {
	ra_speedramp.accel_count += 1;
	ra_speedramp.step_delay=ra_speedramp.step_delay - (((2 * ra_speedramp.step_delay) + ra_speedramp.rest) / (4 * ra_speedramp.accel_count + 1));
	ra_speedramp.rest = ((2 * ra_speedramp.step_delay)+ra_speedramp.rest)%(4 * ra_speedramp.accel_count + 1);
	if (ra_speedramp.step_delay <= ra_speedramp.min_delay) {
	  ra_speedramp.step_delay = ra_speedramp.min_delay;
	  ra_speedramp.run_state = RUN;
	}
	if (ra_speedramp.step_delay > 65535) { 
	  ra_timer.count = (ra_speedramp.step_delay / 65536) + 1; 
	  ra_timer.timer.value = 65536 - (ra_speedramp.step_delay / ra_timer.count); 
	} else {  
	  ra_timer.count = 1; 
	  //if (n > MIN_TICKS) n = MIN_TICKS; 
	  ra_timer.timer.value = 65536 - ra_speedramp.step_delay;
	}
	SETTIMER1(ra_timer.timer);  
      }

     if (ra_speedramp.run_state == DECEL) {
	ra_speedramp.accel_count += 1;
	if (ra_speedramp.accel_count == ra_speedramp.decel_start) {
	  T1CONbits.TMR1ON = 0;
	  UNSETMOTORPROPERTY(ra_status, RUNNING);
	} else {
	  ra_speedramp.step_delay=ra_speedramp.step_delay + (((2 * ra_speedramp.step_delay) + ra_speedramp.rest) / (4 * (ra_speedramp.decel_start - ra_speedramp.accel_count) + 1));
	  ra_speedramp.rest = ((2 * ra_speedramp.step_delay)+ra_speedramp.rest)%(4 * (ra_speedramp.decel_start - ra_speedramp.accel_count) + 1);
	  if (ra_speedramp.step_delay > 65535) { 
	    ra_timer.count = (ra_speedramp.step_delay / 65536) + 1; 
	    ra_timer.timer.value = 65536 - (ra_speedramp.step_delay / ra_timer.count); 
	  } else {  
	    ra_timer.count = 1; 
	    //if (n > MIN_TICKS) n = MIN_TICKS; 
	    ra_timer.timer.value = 65536 - ra_speedramp.step_delay;
	  }
	  SETTIMER1(ra_timer.timer);  
	}
      }

      if (!(GETMOTORPROPERTY(ra_status, HIGHSPEED))) step_inc=1; else step_inc=MICROSTEPS;
      if (GETMOTORPROPERTY(ra_status, BACKWARD)) {
	ra_position -= step_inc; ra_microstep -= step_inc; 
      } else {
	ra_position += step_inc; ra_microstep += step_inc; 
      }
      ra_target_current += step_inc;
      ra_microstep &= (4 * MICROSTEPS) - 1;
      if (!(GETMOTORPROPERTY(ra_status, HIGHSPEED))) {
	ra_pwmindex = ra_microstep & MICROSTEP_MASK;
	if (ra_microstep & PWM_MASK) ra_pwmindex = (MICROSTEPS - 1) - ra_pwmindex;
	SETRAPWMB(pwm_table[ra_pwmindex]); 
	ra_pwmindex = MICROSTEPS - 1 - ra_pwmindex;
	SETRAPWMA(pwm_table[ra_pwmindex]);
      }
      if ((((ra_microstep & WINDINGA_MASK) == 0) || ((ra_microstep & WINDINGA_MASK) == WINDINGA_MASK)) != 0)
	RAPHIA = 1; 
      else 
	RAPHIA = 0;
      if ((ra_microstep & WINDINGB_MASK) != 0) RAPHIB = 1; else  RAPHIB = 0;
    }
    //  Clear TIMER1 interrupt flag;
    PIR1bits.TMR1IF = 0;
  }

  if (INTCONbits.TMR0IF==1) {
     de_timer.current += 1;
    SETTIMER0(de_timer.timer); 
    if (de_timer.current == de_timer.count) {
      de_timer.current=0;
      /* if  ((!(GETMOTORPROPERTY(de_status, SLEWMODE))) && (de_target_current >= de_target)) {
	  T0CONbits.TMR0ON = 0;
	  UNSETMOTORPROPERTY(de_status, RUNNING);
      }
      */
      /* modifier qd highspeed de_target_slow * 8 */
      if  ((!(GETMOTORPROPERTY(de_status, SLEWMODE))) && (de_target_current >= de_target - de_target_slow) && (de_speedramp.run_state != DECEL)) {
        de_speedramp.accel_count = 0;
        de_speedramp.rest = 0;
        if (GETMOTORPROPERTY(de_status, HIGHSPEED))
	  de_speedramp.decel_start = (de_target_slow / 8);
	else
	  de_speedramp.decel_start = de_target_slow;
	de_speedramp.run_state = DECEL;
      }

      if (de_speedramp.run_state == ACCEL) {
	de_speedramp.accel_count += 1;
	de_speedramp.step_delay=de_speedramp.step_delay - (((2 * de_speedramp.step_delay) + de_speedramp.rest) / (4 * de_speedramp.accel_count + 1));
	de_speedramp.rest = ((2 * de_speedramp.step_delay)+de_speedramp.rest)%(4 * de_speedramp.accel_count + 1);
	if (de_speedramp.step_delay <= de_speedramp.min_delay) {
	  de_speedramp.step_delay = de_speedramp.min_delay;
	  de_speedramp.run_state = RUN;
	}
	if (de_speedramp.step_delay > 65535) { 
	  de_timer.count = (de_speedramp.step_delay / 65536) + 1; 
	  de_timer.timer.value = 65536 - (de_speedramp.step_delay / de_timer.count); 
	} else {  
	  de_timer.count = 1; 
	  //if (n > MIN_TICKS) n = MIN_TICKS; 
	  de_timer.timer.value = 65536 - de_speedramp.step_delay;
	}
	SETTIMER0(de_timer.timer);  
      }

     if (de_speedramp.run_state == DECEL) {
	de_speedramp.accel_count += 1;
	if (de_speedramp.accel_count == de_speedramp.decel_start) {
	  T0CONbits.TMR0ON = 0;
	  UNSETMOTORPROPERTY(de_status, RUNNING);
	} else {
	  de_speedramp.step_delay=de_speedramp.step_delay + (((2 * de_speedramp.step_delay) + de_speedramp.rest) / (4 * (de_speedramp.decel_start - de_speedramp.accel_count) + 1));
	  de_speedramp.rest = ((2 * de_speedramp.step_delay)+de_speedramp.rest)%(4 * (de_speedramp.decel_start - de_speedramp.accel_count) + 1);

	  if (de_speedramp.step_delay > 65535) { 
	    de_timer.count = (de_speedramp.step_delay / 65536) + 1; 
	    de_timer.timer.value = 65536 - (de_speedramp.step_delay / de_timer.count); 
	  } else {  
	    de_timer.count = 1; 
	    //if (n > MIN_TICKS) n = MIN_TICKS; 
	  de_timer.timer.value = 65536 - de_speedramp.step_delay;
	  }
	  SETTIMER0(de_timer.timer);  
	}
      }

      if (!(GETMOTORPROPERTY(de_status, HIGHSPEED))) step_inc=1; else step_inc=MICROSTEPS;
      if (GETMOTORPROPERTY(de_status, BACKWARD)) {
	de_position -= step_inc; de_microstep -= step_inc;
      } else {
	de_position += step_inc; de_microstep += step_inc;
      }
      de_target_current += step_inc;
      de_microstep &= (4 * MICROSTEPS) - 1;
      if (!(GETMOTORPROPERTY(de_status, HIGHSPEED))) {
	de_pwmindex = de_microstep & MICROSTEP_MASK;
	if (de_microstep & PWM_MASK) de_pwmindex = (MICROSTEPS - 1) - de_pwmindex;
	SETDEPWMB(pwm_table[de_pwmindex]); 
	de_pwmindex = MICROSTEPS - 1 - de_pwmindex;
	SETDEPWMA(pwm_table[de_pwmindex]);
      }
      if ((((de_microstep & WINDINGA_MASK) == 0) || ((de_microstep & WINDINGA_MASK) == WINDINGA_MASK)) != 0)
	DEPHIA = 1; 
      else 
	DEPHIA = 0;
      if ((de_microstep & WINDINGB_MASK) != 0) DEPHIB = 1; else  DEPHIB = 0;
    }
    //  Clear TIMER0 interrupt flag;
    INTCONbits.TMR0IF = 0;
  }
    
}

/** Low Priority Interrupt Subroutine **/
void low_isr (void) interrupt 2 { // low priority interrupt

   if (PIR1bits.RCIF) {   // Usart Receive
     unsigned char c;
     c=RCREG;
     cmd[usart_index]=c;
     usart_index+=1;
     if (usart_index==32) { send_byte(0x01); usart_index=0;}
     if (c == 0x2e) usart_index=0; // flush '.' ending commands
     if (c == 0x0D) cmd[usart_index - 1] = (c = 0);
     if (c==0) usart_rdy=1;
  }
} 

void main() {
  /** Init interrupts **/
  /* Using priority */
  RCONbits.IPEN=1;
  /* Interrupt priority mask */
  // high priority interrupts
  IPR1bits.TMR1IP=1;
  INTCON2bits.TMR0IP=1;
  //   IPR1bits.TMR2IP=1;
  //   IPR3bits.PTIP=1;
  //   INTCON2bits.RBIP=1;
  // low priority interrupts
  IPR1bits.RCIP=0;
  /* Interrupt Mask */ 
  //   PIE1bits.RCIE=1;
  PIE1bits.TMR1IE=1;
  INTCONbits.TMR0IE=1;
  //  PIE1bits.TMR2IE=1;
  //  PIE3bits.PTIE=1;
  
  INTCONbits.GIE=1; // enable high priority ints
  INTCONbits.PEIE=1; // enable low priority ints
  
  /** Init I/O Ports **/
  RAENABLE=0;
  DEENABLE=0;
  SETRAOUTPUTPORTS;
  SETDEOUTPUTPORTS;
  SETLEDOUTPUTPORTS;
  //SETINPUTPORTS;
  
  /** Init Usart **/
  // usart_open(129); /* 10-> 115200, 32 -> 38400, 64 -> 19200 , 129 -> 9600 @20Mhz */
  usart_open(255); /* 255 -> 9600 @ 40 Mhz */

  while (1) {
    if (usart_rdy) {            // Usart cmd received
      if (cmd[0] != ':') { send_byte('!'); goto next_cmd;}
      switch (cmd[1]) {
	/** Syntrek Protocol **/
      case 'e': 				// Get firmware version
	if (cmd[2] != '1') { goto cant_do; }
	else {send_byte('='); send_string(VERSION);}
	break;
      case 'a': 				// Get number of microsteps per revolution
	if (cmd[2] == '1') {send_byte('='); send_u24(RA_STEPS_360);}
	else if (cmd[2] == '2') { send_byte('='); send_u24(DE_STEPS_360);}
	else goto cant_do;
	break;
      case 'b': 		               // Get number of microsteps per revolution
	if (cmd[2] == '1') {send_byte('='); send_u24(RA_STEPS_WORM);}
	else if (cmd[2] == '2') { send_byte('='); send_u24(DE_STEPS_WORM);}
	else goto cant_do;
	break;
      case 'D': 		               // Get Worm period
	if (cmd[2] == '1') {send_byte('='); send_u24(ra_wormperiod);}
	else if (cmd[2] == '2') { send_byte('='); send_u24(de_wormperiod);}
	else goto cant_do;
	break;
      case 'f':				// Get motor status
	if (cmd[2] == '1') {
	  send_byte('='); send_u12(ra_status);
	} else if (cmd[2] == '2') { 
	  send_byte('='); send_u12(de_status);
	}
	else goto cant_do;
	break;
      case 'j': 		               // Get encoder values
	if (cmd[2] == '1') {send_byte('='); send_u24(ra_position);}
	else if (cmd[2] == '2') { send_byte('='); send_u24(de_position);}
	else goto cant_do;
	break;
      case 'g':				// get high speed ratio
	if (cmd[2] == '1') {send_byte('='); send_u24(0x000008);}
	else if (cmd[2] == '2') { send_byte('='); send_u24(0x000008);}
	else goto cant_do;
	break;
      case 'E':                                          	// Set encoder values
	if (cmd[2] == '1') {ra_position=get_u24();send_byte('=');}
	else if (cmd[2] == '2') { de_position=get_u24();send_byte('=');}
	else goto cant_do;
	break;
      case 'F':                                          	// Initialize & activate motors
	if ((cmd[2] != '1') && (cmd[2] != '2') && (cmd[2] != '3')) goto cant_do;
	init_pwm();	
	init_timers();
	if ((cmd[2] == '1') || (cmd[2] == '3')) { 
	  SETRAPWMA(pwm_table[ra_pwmindex]); SETRAPWMB(pwm_table[MICROSTEPS - 1 - ra_pwmindex]);
	  SETRAPHIA(ra_microstep); SETRAPHIB(ra_microstep);
          SETMOTORPROPERTY(ra_status, INITIALIZED);
	  RAENABLE = 1;
	}
	if ((cmd[2] == '2') || (cmd[2] == '3')) { 
	  SETDEPWMA(pwm_table[de_pwmindex]); SETDEPWMB(pwm_table[MICROSTEPS - 1 - de_pwmindex]);
	  SETDEPHIA(de_microstep); SETDEPHIB(de_microstep);
          SETMOTORPROPERTY(de_status, INITIALIZED);
	  DEENABLE = 1;
	}
	send_byte('=');
	break;

      case 'J':						// Start motor
	if (cmd[2] == '1')  {ra_resume(); send_byte('='); }
	else if (cmd[2] == '2')  {de_resume(); send_byte('='); }
	else goto cant_do;
	break;
      case 'K':  						// Stop motor
	if (cmd[2] == '1')  {ra_pause(); send_byte('='); }
	else if (cmd[2] == '2')  {de_pause(); send_byte('=');}
	else goto cant_do;
	break;
      case 'L':  						// Instant Stop motor
	if (cmd[2] == '1')  {ra_stop(); send_byte('='); }
	else if (cmd[2] == '2')  {de_stop(); send_byte('=');}
	else goto cant_do;
	break;
      case 'I':						// Set Speed
	if (cmd[2] == '1') { ra_wormperiod=get_u24(); if (GETMOTORPROPERTY(ra_status, RUNNING)) compute_timer_ra(ra_wormperiod);send_byte('='); }
	else if (cmd[2] == '2') { de_wormperiod=get_u24(); if (GETMOTORPROPERTY(de_status, RUNNING)) compute_timer_de(de_wormperiod);send_byte('='); }
	else goto cant_do;
	break;
      case 'G':						// Set Mode/Direction
	if ((cmd[2] != '1') && (cmd[2] != '2')) goto cant_do;
	if (cmd[2] == '1') {
	  unsigned char modedir=get_u8(); 
	  if (modedir&0x0F) SETMOTORPROPERTY(ra_status, BACKWARD); else UNSETMOTORPROPERTY(ra_status, BACKWARD);
	  modedir=(modedir>>4); 
	  if (modedir==0) { UNSETMOTORPROPERTY(ra_status, SLEWMODE); SETMOTORPROPERTY(ra_status, HIGHSPEED);}
	  else if (modedir==1) { SETMOTORPROPERTY(ra_status, SLEWMODE); UNSETMOTORPROPERTY(ra_status, HIGHSPEED); }
	  else if (modedir==2) { UNSETMOTORPROPERTY(ra_status, SLEWMODE); UNSETMOTORPROPERTY(ra_status, HIGHSPEED);}
	  else if (modedir==3) { SETMOTORPROPERTY(ra_status, SLEWMODE); SETMOTORPROPERTY(ra_status, HIGHSPEED); }
	}
	if (cmd[2] == '2') {
	  unsigned char modedir=get_u8(); 
	  if (modedir&0x0F) SETMOTORPROPERTY(de_status, BACKWARD); else UNSETMOTORPROPERTY(de_status, BACKWARD);
	  modedir=(modedir>>4); 
	  if (modedir==0) { UNSETMOTORPROPERTY(de_status, SLEWMODE); SETMOTORPROPERTY(de_status, HIGHSPEED);}
	  else if (modedir==1) { SETMOTORPROPERTY(de_status, SLEWMODE); UNSETMOTORPROPERTY(de_status, HIGHSPEED); }
	  else if (modedir==2) { UNSETMOTORPROPERTY(de_status, SLEWMODE); UNSETMOTORPROPERTY(de_status, HIGHSPEED);}
	  else if (modedir==3) { SETMOTORPROPERTY(de_status, SLEWMODE); SETMOTORPROPERTY(de_status, HIGHSPEED); }
	 }
	 send_byte('='); 
	break;
      case 'H':                                          	// Set Goto Target
	if (cmd[2] == '1') {ra_target=get_u24();send_byte('=');}
	else if (cmd[2] == '2') { de_target=get_u24();send_byte('=');}
	else goto cant_do;
	break;
      case 'M':                                          	// Set Goto BreakSteps
	if (cmd[2] == '1') {ra_target_slow=get_u24();send_byte('=');}
	else if (cmd[2] == '2') { de_target_slow=get_u24();send_byte('=');}
	else goto cant_do;
	break;
      case 'U':                                          	// Set BreakSteps
	if (cmd[2] == '1') {ra_breaks=get_u24();send_byte('=');}
	else if (cmd[2] == '2') { de_breaks=get_u24();send_byte('=');}
	else goto cant_do;
	break;

      default: goto cant_do;
      }
      goto next_cmd;
    cant_do: send_byte('!'); // Can't execute command 
    next_cmd: send_byte('\x0d'); usart_index=0,usart_rdy=0;
    } // Usart received 
  }
}
